#include "OccupancyProbabilityGrid.h"
#include "stdafx.h"

OccupancyProbabilityGrid::OccupancyProbabilityGrid(int world_size, float initial_probability) {
	row_size = world_size;
	corner = -row_size/2.0f;
	grid = new float[world_size*world_size];

	for (int i = 0; i < world_size*world_size; i++) {
		grid[i] = initial_probability;
	}
}


OccupancyProbabilityGrid::~OccupancyProbabilityGrid(void) {
	delete [] grid;
}

float OccupancyProbabilityGrid::at(int row, int col) {
	return grid[row * row_size + col];
}

void OccupancyProbabilityGrid::set(int row, int col, float val) {
	grid[row * row_size + col] = val;
}

void OccupancyProbabilityGrid::update(occgrid_t* occgrid){
	World* w = World::instance();
	
	std::cout << occgrid->topCorner[0] << " " << occgrid->topCorner[1] << endl;
	int startRow = occgrid->topCorner[0] - corner,
		startCol = occgrid->topCorner[1] - corner;
	float currentBelief = 0;
	float pNumerator = 0;
	float pDenominator = 0;
	bool error = false;

	for(int i = 0; i < occgrid->dimensions[0]; i ++) {
		for(int j = 0; j < occgrid->dimensions[1]; j ++) {
			error = false;
			currentBelief = at(startRow + i, startCol +j);
			
			//error = checkForError(occgrid, i, j);

			char tmp = occgrid->grid[i][j];

			if(error) {
				if(tmp == '1') tmp = '0';
				else tmp = '1';
			}

			if(tmp == '1') {
				pDenominator = w->getTruePositive() * currentBelief + (1.0f - w->getTruePositive())*(1.0f-currentBelief);
				pNumerator = w->getTruePositive() * currentBelief;	
			}
			else {
				pDenominator = (1.0f - w->getTrueNegative()) * currentBelief + w->getTrueNegative()*(1.0f-currentBelief);
				pNumerator = (1.0f - w->getTrueNegative()) * currentBelief;
			}

			currentBelief = pNumerator/pDenominator;

			grid[(startRow + i) * row_size + j + startCol] = currentBelief;
		}
	}
}

bool OccupancyProbabilityGrid::checkForError(occgrid_t* occgrid, int row, int col){
	int neighbors[8][2];
	bool connected[8];
	char current = occgrid->grid[row][col];

	neighbors[0][0] = neighbors[1][0] = neighbors[2][0] = neighbors[3][0] = neighbors[4][0] = neighbors[5][0] = neighbors[6][0] = neighbors[7][0] = -1;
	neighbors[0][1] = neighbors[1][1] = neighbors[2][1] = neighbors[3][1] = neighbors[4][1] = neighbors[5][1] = neighbors[6][1] = neighbors[7][1] = -1;
	connected[0] = connected[1] = connected[2] = connected[3] = connected[4] = connected[5] = connected[6] = connected[7] = false;

	//if(col == 0 || row == 0 || col == occgrid->dimensions[0]-1 || row  == occgrid->dimensions[0]-1)
	//	return false;

	if(col > 0){
		neighbors[0][0] = col-1;
		neighbors[0][1] = row;

		if(row > 0){
			neighbors[4][0] = col-1;
			neighbors[4][1] = row-1;
		}
		if(row < occgrid->dimensions[0]-1){
			neighbors[5][0] = col-1;
			neighbors[5][1] = row+1;
		}
	}
	if(col < occgrid->dimensions[1]-1){
		neighbors[1][0] = col+1;
		neighbors[1][1] = row;

		if(row > 0){
			neighbors[6][0] = col+1;
			neighbors[6][1] = row-1;
		}
		if(row < occgrid->dimensions[0]-1){
			neighbors[7][0] = col+1;
			neighbors[7][1] = row+1;
		}
	}
	if(row > 0){
		neighbors[2][0] = col;
		neighbors[2][1] = row-1;
	}
	if(row < occgrid->dimensions[0]-1){
		neighbors[3][0] = col;
		neighbors[3][1] = row+1;
	}
	
	int side=0, diag=0;
	for(int i = 0; i < 4; i ++){
		if(neighbors[i][0] != -1){
			if(current == occgrid->grid[neighbors[i][1]][neighbors[i][0]]){
				connected[i] = true;
				side++;
			}
		}
	}
	for(int i = 0; i < 4; i ++){
		if(neighbors[i+4][0] != -1){
			if(current == occgrid->grid[neighbors[i+4][1]][neighbors[i+4][0]]){
				connected[i] = true;
				diag++;
			}
		}
	}
	if((connected[0] && connected[2]) || 
		(connected[0] && connected[3]) || 
		(connected[1] && connected[2]) || 
		(connected[1] && connected[3]) ||
		(connected[0] && connected[1])) return false;
	return true;
}

void OccupancyProbabilityGrid::cleanup(){
	for (int i = 0; i < row_size; i ++){
		for(int j = 0; j < row_size; j++) {
			if(grid[i*row_size+j] >.15 && grid[i] < .95){
				if(obstacleInDirection(Vector2(j, i), -1, 0) &&
				   obstacleInDirection(Vector2(j, i), 1, 0) &&
				   obstacleInDirection(Vector2(j, i), 0, -1) &&
			 	   obstacleInDirection(Vector2(j, i), 0, 1))
					grid[i*row_size+j] = 1.0;

				else
					grid[i*row_size+j] = 0.0;
			}
		}
	}
}

bool OccupancyProbabilityGrid::obstacleInDirection(Vector2 current, int x, int y){
	int curx = current.x, cury = current.y;
	while(grid[cury*row_size+curx] < .95){
		if(grid[cury*row_size+curx] < .15)
			return false;
		curx += x;
		cury += y;
	}
	return true;
}